/*************************************************
 *  Object:       Tric! A flying Ricotta.
 *  Authors:      Teo (matteo.piva-gmail.com)
                  Carletto (carlo.checchi-gmail.com)
*************************************************/

#include "config.h"
#include "motors.h"


void MOTORS_update(unsigned short *MOTORS_speed) {
     unsigned short cnt;
     unsigned short MOTORS_biggest_speed;

     MOTORS_biggest_speed = 0;

        for(cnt=0; cnt<4; cnt++) {
            if(MOTORS_speed[cnt] > MOTORS_max_speed) MOTORS_speed[cnt] = MOTORS_max_speed;
            MOTORS_biggest_speed = (MOTORS_speed[cnt] >= MOTORS_biggest_speed) ? MOTORS_speed[cnt] : MOTORS_biggest_speed;
        }

        for(cnt=0; cnt<=MOTORS_biggest_speed; cnt++) {
            #ifndef DEBUG
            if(cnt == MOTORS_speed[0]) MOTORS_M1 = 1;
            if(cnt == MOTORS_speed[1]) MOTORS_M2 = 1;
            if(cnt == MOTORS_speed[2]) MOTORS_M3 = 1;
            if(cnt == MOTORS_speed[3]) MOTORS_MS = 1;
            #endif
            Delay_us(2);
        }
}

/* ESC startup sequence
 * Sends to ESC throttle down for 4s
 */
void MOTORS_esc_startup() {
     unsigned short cnt=0;
     while(cnt<100) {
         MOTORS_M1 = 0;
         MOTORS_M2 = 0;
         MOTORS_M3 = 0;
         MOTORS_MS = 0;
         Delay_ms(1);
         MOTORS_M1 = 1;
         MOTORS_M2 = 1;
         MOTORS_M3 = 1;
         MOTORS_MS = 1;
         Delay_ms(20);
         cnt++;
     }
}



/* ESC configuration sequence
 * Sends to ESC throttle extremes
 * (all pulled down and all pulled up)
 */

void MOTORS_esc_config() {
    unsigned short MOTORS_speed_align[4];
    unsigned short cnt=0;
    MOTORS_speed_align[0] = MOTORS_max_speed;
    MOTORS_speed_align[1] = MOTORS_max_speed;
    MOTORS_speed_align[2] = MOTORS_max_speed;
    MOTORS_speed_align[3] = MOTORS_max_speed;
    while(cnt<100) {
         MOTORS_M1 = 0;
         MOTORS_M2 = 0;
         MOTORS_M3 = 0;
         MOTORS_MS = 0;
         Delay_ms(1);
         MOTORS_update(&MOTORS_speed_align);
         MOTORS_M1 = 1;
         MOTORS_M2 = 1;
         MOTORS_M3 = 1;
         MOTORS_MS = 1;
         Delay_ms(20);
         cnt++;
    }
    cnt = 0;
    while(cnt<100) {
         MOTORS_M1 = 0;
         MOTORS_M2 = 0;
         MOTORS_M3 = 0;
         MOTORS_MS = 0;
         Delay_ms(1);
         MOTORS_M1 = 1;
         MOTORS_M2 = 1;
         MOTORS_M3 = 1;
         MOTORS_MS = 1;
         Delay_ms(20);
         cnt++;
    }
    Delay_ms(2000);
}